﻿#pragma once

#include <cmath>
#include <iostream>
#include <numeric>
#include <optional>
#include <random>

#include <learn/check.h>
#include <learn/dll_ext.h>
#include <learn/math/math.h>
#include <learn/utils/json.h>

namespace xi
{

namespace geom
{

template <std::size_t M, std::size_t N, typename T> struct MatMxN
{
    T data[M * N];

    /**
     * @brief 基本空构造
     *
     */
    MatMxN(T t = T{})
    {
        for (std::size_t i = 0; i < M * N; i++)
            data[i] = t;
    }

    /**
     * @brief 变长构造
     *
     * @tparam Ts
     * @return requires(... == N && (std::is_same_v<Ts, T> && ...)) VecT(const Ts&... ts)
     */
    template <class... Ts>
        requires(sizeof...(Ts) == M * N && (std::is_same_v<Ts, T> && ...))
    explicit MatMxN(const Ts &...ts) : data{ts...}
    {
    }

    /**
     * @brief 拷贝构造
     *
     * @param[in] rhs
     */
    MatMxN(const MatMxN &rhs)
    {
        std::copy(rhs.data, rhs.data + M * N, data);
    }

    /**
     * @brief 拷贝赋值
     *
     * @param[in] rhss
     * @return MatMxN&
     */
    const MatMxN &operator=(const MatMxN &rhs)
    {
        if (this != &rhs) [[likely]]
            std::copy(rhs.data, rhs.data + M * N, data);
        return *this;
    }

    /**
     * @brief 取反
     *
     * @return MatMxN
     */
    MatMxN operator-() const
    {
        MatMxN res;
        for (std::size_t i = 0; i < M * N; i++)
            res.data[i] = -data[i];
        return res;
    }

    /**
     * @brief 获得元素
     *
     * @param[in] i
     * @param[in] j
     * @return T
     */
    T operator()(std::size_t i, std::size_t j) const
    {
        ASSERT_INFO(i < M && j < N, "Index out of range");
        return data[i * N + j];
    }

    /**
     * @brief 获得元素
     *
     * @param[in] i
     * @param[in] j
     * @return T&
     */
    T &operator()(std::size_t i, std::size_t j)
    {
        ASSERT_INFO(i < M && j < N, "Index out of range");
        return data[i * N + j];
    }

#define MAT_BINARY_OP(func, op)                                                                                        \
    MatMxN func(T rhs) const                                                                                           \
    {                                                                                                                  \
        MatMxN res;                                                                                                    \
        for (std::size_t i = 0; i < M; i++)                                                                            \
            for (std::size_t j = 0; j < N; j++)                                                                        \
                res(i, j) = data[i * N + j] op rhs;                                                                    \
        return res;                                                                                                    \
    }

    MAT_BINARY_OP(operator+, +)
    MAT_BINARY_OP(operator-, -)
    MAT_BINARY_OP(operator*, *)
    MAT_BINARY_OP(operator/, /)
#undef MAT_BINARY_OP

#define MAT_BINARY_OP(func, op)                                                                                        \
    MatMxN func(const MatMxN &rhs) const                                                                               \
    {                                                                                                                  \
        MatMxN res;                                                                                                    \
        for (std::size_t i = 0; i < M; i++)                                                                            \
            for (std::size_t j = 0; j < N; j++)                                                                        \
                res(i, j) = data[i * N + j] op rhs(i, j);                                                              \
        return res;                                                                                                    \
    }

    MAT_BINARY_OP(operator+, +)
    MAT_BINARY_OP(operator-, -)
#undef MAT_BINARY_OP

#define MAT_BINARY_OP(func, op)                                                                                        \
    const MatMxN &func(T rhs)                                                                                          \
    {                                                                                                                  \
        for (std::size_t i = 0; i < M; i++)                                                                            \
            for (std::size_t j = 0; j < N; j++)                                                                        \
                data[i * N + j] op rhs;                                                                                \
        return *this;                                                                                                  \
    }

    MAT_BINARY_OP(operator+=, +=)
    MAT_BINARY_OP(operator-=, -=)
    MAT_BINARY_OP(operator*=, *=)
    MAT_BINARY_OP(operator/=, /=)
#undef MAT_BINARY_OP

#define MAT_BINARY_OP(func, op)                                                                                        \
    const MatMxN &func(const MatMxN &rhs)                                                                              \
    {                                                                                                                  \
        for (std::size_t i = 0; i < M; i++)                                                                            \
            for (std::size_t j = 0; j < N; j++)                                                                        \
                data[i * N + j] op rhs(i, j);                                                                          \
        return *this;                                                                                                  \
    }

    MAT_BINARY_OP(operator+=, +=)
    MAT_BINARY_OP(operator-=, -=)
#undef MAT_BINARY_OP

    /**
     * @brief 矩阵乘法
     *
     * @tparam P
     * @param[in] mat
     * @return MatMxN<M, P, T>
     */
    template <std::size_t P> MatMxN<M, P, T> operator*(const MatMxN<N, P, T> &mat) const
    {
        MatMxN<M, P, T> matn2;
        for (std::size_t i = 0; i < M; i++)
            for (std::size_t j = 0; j < P; j++)
                for (std::size_t k = 0; k < N; k++)
                    matn2(i, j) += data[i * N + k] * mat(k, j);
        return matn2;
    }

    /**
     * @brief 指标返回
     *
     * @param[in] i
     * @return T
     */
    T operator[](std::size_t i) const
    {
        ASSERT_INFO(i < M * N, "Index out of range");
        return data[i];
    }

    /**
     * @brief 指标返回
     *
     * @param[in] i
     * @return T&
     */
    T &operator[](std::size_t i)
    {
        ASSERT_INFO(i < M * N, "Index out of range");
        return data[i];
    }

    /**
     * @brief 转置
     *
     * @return MatMxN<N, M, T>
     */
    MatMxN<N, M, T> transposed() const
    {
        MatMxN<N, M, T> res;
        for (std::size_t i = 0; i < M; i++)
            for (std::size_t j = 0; j < N; j++)
                res(j, i) = data[i * N + j];
        return res;
    }

    /**
     * @brief 取模
     *
     * @return T
     */
    T modulus() const
    {
        return std::sqrt(square_modulus());
    }

    /**
     * @brief 模平方
     *
     * @return T
     */
    T square_modulus() const
    {
        T r{};
        for (std::size_t i = 0; i < M * N; i++)
            r += data[i] * data[i];
        return r;
    }

    /**
     * @brief 正则化.
     * @see
     */
    MatMxN &normalize()
    {
        T mod = modulus();
        ASSERT_INFO(mod > std::numeric_limits<T>::epsilon(), "Divide by zero");
        for (std::size_t i = 0; i < M * N; i++)
            data[i] /= mod;
        return *this;
    }

    /**
     * @brief 返回正则化
     *
     * @return MatMxN
     */
    MatMxN normalized() const
    {
        T mod = modulus();
        ASSERT_INFO(mod > std::numeric_limits<T>::epsilon(), "Divide by zero");
        MatMxN res;
        for (std::size_t i = 0; i < M * N; i++)
            res.data[i] = data[i] / mod;
        return res;
    }

    /**
     * @brief 绝对值
     *
     * @return MatMxN&
     */
    MatMxN &absolute()
    {
        for (std::size_t i = 0; i < M * N; i++)
            data[i] = std::abs(data[i]);
        return *this;
    }

    /**
     * @brief 返回绝对值向量
     *
     * @return VecT
     */
    MatMxN absoluted() const
    {
        MatMxN res;
        for (std::size_t i = 0; i < M * N; i++)
            res[i] = std::abs(data[i]);
        return res;
    }

    /**
     * @brief 到点的距离
     *
     * @param[in] mat
     * @return T
     */
    T distance(const MatMxN &mat) const
    {
        return std::sqrt(square_distance(mat));
    }

    /**
     * @brief 到点的距离平方
     *
     * @param[in] mat
     * @return T
     */
    T square_distance(const MatMxN &mat) const
    {
        return (*this - mat).square_modulus();
    }

    /**
     * @brief 比较运算
     *
     * @param[in] mat
     * @return true
     * @return false
     */
    bool operator==(const MatMxN &mat) const
    {
        return distance(mat) < math::Numeric::distance;
    }

#define MAT_VAL_OP(func, i)                                                                                            \
    T func() const                                                                                                     \
    {                                                                                                                  \
        static_assert(i < M * N, "Index out of range");                                                                \
        return data[i];                                                                                                \
    }                                                                                                                  \
    T &func()                                                                                                          \
    {                                                                                                                  \
        static_assert(i < M * N, "Index out of range");                                                                \
        return data[i];                                                                                                \
    }

    MAT_VAL_OP(x, 0)
    MAT_VAL_OP(y, 1)
    MAT_VAL_OP(z, 2)
    MAT_VAL_OP(w, 3)
    MAT_VAL_OP(r, 0)
    MAT_VAL_OP(g, 1)
    MAT_VAL_OP(b, 2)
    MAT_VAL_OP(a, 3)
    MAT_VAL_OP(front, 0)
    MAT_VAL_OP(back, M *N - 1)
#undef MAT_VAL_OP

    /**
     * @brief 序列化
     *
     * @return utils::Json
     */
    utils::Json dump_json() const
    {
        static constexpr std::size_t row = N == 1 ? 1 : M;
        static constexpr std::size_t col = N == 1 ? M : N;

        utils::Json json;
        if constexpr (row == 1)
        {
            for (std::size_t j = 0; j < col; j++)
                json.append(this->data[j]);
            return json;
        }
        else
        {
            json["type"] = "mat";
            utils::Json data;
            for (std::size_t i = 0; i < row; i++)
            {
                utils::Json row;
                for (std::size_t j = 0; j < col; j++)
                    row.append(this->data[i * col + j]);
                data.append(std::move(row));
            }
            json["row"] = int(row);
            json["col"] = int(col);
            json["data"] = std::move(data);
            return json;
        }
    }

    /**
     * @brief 输出函数.
     * @return
     * @param os
     * @param v
     * @see
     */
    friend std::ostream &operator<<(std::ostream &os, const MatMxN &mat)
    {
        static constexpr std::size_t row = N == 1 ? 1 : M;
        static constexpr std::size_t col = N == 1 ? M : N;

        os << "{ ";
        if constexpr (row > 1)
            std::cout << std::endl;

        for (std::size_t i = 0; i < row; i++)
        {
            for (std::size_t j = 0; j < col; j++)
                os << mat[i * col + j] << " ";

            if constexpr (row > 1)
                std::cout << std::endl;
        }
        os << "}";
        return os;
    }

    /**
     * @brief 单位阵
     *
     * @return MatMxN
     */
    static MatMxN identity()
    {
        static_assert(M == N, "Identity matrix must be square");
        MatMxN res;
        for (std::size_t i = 0; i < M; ++i)
            res(i, i) = 1;
        return res;
    }
};

template <typename T> using Mat4x4 = MatMxN<4, 4, T>;
template <typename T> using Mat4x3 = MatMxN<4, 3, T>;
template <typename T> using Mat4x2 = MatMxN<4, 2, T>;
template <typename T> using Mat4x1 = MatMxN<4, 1, T>;

template <typename T> using Mat3x4 = MatMxN<3, 4, T>;
template <typename T> using Mat3x3 = MatMxN<3, 3, T>;
template <typename T> using Mat3x2 = MatMxN<3, 2, T>;
template <typename T> using Mat3x1 = MatMxN<3, 1, T>;

template <typename T> using Mat2x4 = MatMxN<2, 4, T>;
template <typename T> using Mat2x3 = MatMxN<2, 3, T>;
template <typename T> using Mat2x2 = MatMxN<2, 2, T>;
template <typename T> using Mat2x1 = MatMxN<2, 1, T>;

template <typename T> using Mat1x4 = MatMxN<1, 4, T>;
template <typename T> using Mat1x3 = MatMxN<1, 3, T>;
template <typename T> using Mat1x2 = MatMxN<1, 2, T>;
template <typename T> using Mat1x1 = MatMxN<1, 1, T>;

template <std::size_t N, typename T> using VecT = MatMxN<N, 1, T>;

// 常用类型定义
template <typename T> using Vec2T = VecT<2, T>;
template <typename T> using Vec3T = VecT<3, T>;
template <typename T> using Vec4T = VecT<4, T>;

// 高精度版本
using Vec2 = Vec2T<double>;
using Vec3 = Vec3T<double>;
using Vec4 = Vec4T<double>;

// 浮点版本
using Vec2f = Vec2T<float>;
using Vec3f = Vec3T<float>;
using Vec4f = Vec4T<float>;

// 整型版本
using Vec2i = Vec2T<int>;
using Vec3i = Vec3T<int>;
using Vec4i = Vec4T<int>;

// 无符号整型版本
using Vec2u = Vec2T<std::size_t>;
using Vec3u = Vec3T<std::size_t>;
using Vec4u = Vec4T<std::size_t>;

#define MAT_OP(func, op)                                                                                               \
    template <std::size_t N, class T> inline VecT<N, T> func(const VecT<N, T> &v1, const VecT<N, T> &v2)               \
    {                                                                                                                  \
        VecT<N, T> res;                                                                                                \
        for (std::size_t i = 0; i < N; i++)                                                                            \
            res[i] = v1[i] op v2[i];                                                                                   \
        return res;                                                                                                    \
    }

MAT_OP(operator*, *)
MAT_OP(operator/, /)

#undef MAT_OP

#define MAT_OP(func, op)                                                                                               \
    template <std::size_t N, class T> inline const VecT<N, T> &func(VecT<N, T> &v1, const VecT<N, T> &v2)              \
    {                                                                                                                  \
        for (std::size_t i = 0; i < N; i++)                                                                            \
            v1[i] op v2[i];                                                                                            \
        return v1;                                                                                                     \
    }

MAT_OP(operator*=, *=)
MAT_OP(operator/=, /=)

#undef MAT_OP

/**
 * @brief 类型转换
 *
 * @tparam N
 * @tparam U
 * @tparam V
 * @param[in] v
 * @return VecT<N, U>
 */
template <class U, std::size_t N, class V> inline VecT<N, U> convert(const VecT<N, V> &v)
{
    VecT<N, U> res;
    for (std::size_t i = 0; i < N; i++)
        res[i] = static_cast<U>(v[i]);
    return res;
}

/**
 * @brief 内积运算
 *
 * @tparam N
 * @tparam T
 * @param[in] v1
 * @param[in] v2
 * @return T
 */
template <std::size_t N, class T> inline T dot(const VecT<N, T> &v1, const VecT<N, T> &v2)
{
    T r{};
    for (std::size_t i = 0; i < N; i++)
        r += v1[i] * v2[i];
    return r;
}

/**
 * @brief 叉积
 *
 * @tparam T
 * @param[in] v1
 * @param[in] v2
 * @return VecT<3, T>
 */
template <class T> inline VecT<3, T> cross(const VecT<3, T> &v1, const VecT<3, T> &v2)
{
    return VecT<3, T>{v1.y() * v2.z() - v1.z() * v2.y(), v1.z() * v2.x() - v1.x() * v2.z(),
                      v1.x() * v2.y() - v1.y() * v2.x()};
}

/**
 * @brief 返回正交向量
 *
 * @tparam N
 * @tparam T
 * @param[in] v
 * @return VecT<N, T>
 */
template <std::size_t N, class T> inline VecT<N, T> orthogonal_vec(const VecT<N, T> &v)
{
    // 巨坑：有些版本编译器不支持将 static_assert(false) 写在 else 里
    static_assert(N <= 3, "orthogonal_vec is not defined for N > 3");
    if constexpr (N == 2)
        return Vec2T<T>{-v.y(), v.x()};
    else if constexpr (N == 3)
    {
        if (std::abs(v.x()) < math::Numeric::distance)
            return Vec3T<T>{T{}, -v.z(), v.y()};
        else if (std::abs(v.y()) < math::Numeric::distance)
            return Vec3T<T>{v.z(), T{}, -v.x()};
        else if (std::abs(v.z()) < math::Numeric::distance)
            return Vec3T<T>{-v.y(), v.x(), T{}};
        return Vec3T<T>{T{}, -v.z(), v.y()};
    }
}

/**
 * @brief 笛卡尔坐标转齐次坐标
 *
 * @tparam N
 * @tparam T
 * @param[in] v
 * @param[in] w
 * @return VecT<N + 1, T>
 */
template <std::size_t N, class T> inline VecT<N + 1, T> cartesian_to_homogenous(const VecT<N, T> &v, T w = 1)
{
    VecT<N + 1, T> p;
    std::copy(v.data, v.data + N, p.data);
    p[N] = w;
    return p;
}

/**
 * @brief 齐次坐标转笛卡尔坐标
 *
 * @tparam N
 * @tparam T
 * @param[in] p
 * @return VecT<N, T>
 */
template <std::size_t N, class T> inline VecT<N - 1, T> homogenous_to_cartesian(const VecT<N, T> &p)
{
    static_assert(N > 1, "homogenous_to_cartesian is not defined for N < 2");
    VecT<N - 1, T> v;
    std::copy(p.data, p.data + N - 1, v.data);
    return v / p[N - 1];
}

/**
 * @brief 生成 [x0,x1] x [x0,x1] x ... 方形区域内的随机点
 *
 * @tparam N
 * @tparam T
 * @param[in] x0
 * @param[in] x1
 * @return VecT<N, T>
 */
template <std::size_t N, class T> inline VecT<N, T> random_point(T x0, T x1)
{
    static std::mt19937 generator(std::random_device{}());
    if constexpr (std::is_floating_point_v<T>)
    {
        static std::uniform_real_distribution<T> uniform(x0, x1);
        VecT<N, T> p;
        std::generate(p.data, p.data + N, [&]() { return uniform(generator); });
        return p;
    }
    else
    {
        static std::uniform_int_distribution<T> uniform(x0, x1);
        VecT<N, T> p;
        std::generate(p.data, p.data + N, [&]() { return uniform(generator); });
        return p;
    }
}

/**
 * @brief 求逆
 *
 * @return MatMxN<N, N, T>
 */
template <std::size_t N, class T> inline MatMxN<N, N, T> invert(const MatMxN<N, N, T> &mat)
{
    // 初始化单位阵
    auto m = MatMxN<N, N, T>::identity();
    auto old = mat;

    T max;             // 最大值
    std::size_t index; // 最大值位置

    for (std::size_t i = 0; i < N; i++)
    {
        max = T{};
        index = i;

        // 寻找最大列元作为主元
        for (std::size_t j = i; j < N; j++)
        {
            T tmp = std::abs(old(j, i));
            if (tmp > max)
            {
                max = tmp;
                index = j;
            }
        }

        ASSERT_INFO(max > std::numeric_limits<T>::epsilon(), "Matrix not invertible");

        // 交换主元
        for (std::size_t q = 0; q < N; q++)
        {
            std::swap(old(index, q), old(i, q));
            std::swap(m(index, q), m(i, q));
        }

        // Gauss 消去
        for (std::size_t j = 0; j < N; j++)
        {
            T tmp = old(i, i);
            if (i == j)
            {
                for (std::size_t k = 0; k < N; k++)
                {
                    m(j, k) /= tmp;
                    old(j, k) /= tmp;
                }
                continue;
            }
            tmp = old(j, i) / old(i, i);
            for (std::size_t k = 0; k < N; k++)
            {
                m(j, k) -= tmp * m(i, k);
                old(j, k) -= tmp * old(i, k);
            }
        }
    }
    return m;
}

/**
 * @brief 平移矩阵
 *
 * @tparam T
 * @param[in] d
 * @return MatMxN<4, 4, T>
 */
template <typename T> inline MatMxN<4, 4, T> translate(const MatMxN<3, 1, T> &d)
{
    auto A = MatMxN<4, 4, T>::identity();

    A(0, 3) = d.x();
    A(1, 3) = d.y();
    A(2, 3) = d.z();

    return A;
}

/**
 * @brief 伸缩矩阵
 *
 * @tparam T
 * @param[in] scale
 * @return MatMxN<4, 4, T>
 */
template <typename T> inline MatMxN<4, 4, T> scale(const MatMxN<3, 1, T> &scale)
{
    auto S = MatMxN<4, 4, T>::identity();
    S(0, 0) = scale.x();
    S(1, 1) = scale.y();
    S(2, 2) = scale.z();
    return S;
}

/**
 * @brief 旋转矩阵
 *
 * @tparam T
 * @param[in] axis
 * @param[in] angle
 * @return MatMxN<4, 4, T>
 */
template <typename T> inline MatMxN<4, 4, T> rotate(const MatMxN<3, 1, T> &axis, T angle)
{
    auto kx = axis.x(), ky = axis.y(), kz = axis.z();
    auto c = std::cos(angle), s = std::sin(angle);

    MatMxN<4, 4, T> R;
    R(0, 0) = kx * kx * (1 - c) + c;
    R(0, 1) = kx * ky * (1 - c) - kz * s;
    R(0, 2) = kx * kz * (1 - c) + ky * s;
    R(1, 0) = kx * ky * (1 - c) + kz * s;
    R(1, 1) = ky * ky * (1 - c) + c;
    R(1, 2) = ky * kz * (1 - c) - kx * s;
    R(2, 0) = kx * kz * (1 - c) - ky * s;
    R(2, 1) = ky * kz * (1 - c) + kx * s;
    R(2, 2) = kz * kz * (1 - c) + c;
    R(3, 3) = 1;
    return R;
}

/**
 * @brief 计算正交投影矩阵
 *
 * @tparam T
 * @param[in] left
 * @param[in] right
 * @param[in] bottom
 * @param[in] top
 * @param[in] zNear
 * @param[in] zFar
 * @return MatMxN<4, 4, T>
 */
template <typename T> inline MatMxN<4, 4, T> ortho(T left, T right, T bottom, T top, T zNear, T zFar)
{
    auto A = MatMxN<4, 4, T>::identity();

    A(0, 0) = 2 / (right - left);
    A(1, 1) = 2 / (top - bottom);
    A(2, 2) = -2 / (zFar - zNear);

    A(0, 3) = -(left + right) / (right - left);
    A(1, 3) = -(top + bottom) / (top - bottom);
    A(2, 3) = (zFar + zNear) / -(zFar - zNear);

    return A;
}

/**
 * @brief 计算透视投影矩阵
 *
 * @tparam T
 * @param[in] fovy 视角度数
 * @param[in] aspect 横纵比
 * @param[in] zNear 最近面
 * @param[in] zFar 最远面
 * @return MatMxN<4, 4, T>
 */
template <typename T> inline MatMxN<4, 4, T> perspective(T fovy, T aspect, T zNear, T zFar)
{
    auto A = MatMxN<4, 4, T>::identity();

    // 将平截头体映射为长方体
    A(0, 0) = zNear;
    A(1, 1) = zNear;
    A(2, 2) = zNear + zFar;
    A(2, 3) = zNear * zFar;
    A(3, 2) = -1;
    A(3, 3) = 0;

    // 做正交投影
    T bottom = -zNear * std::tan(math::degrees_to_radians(fovy / 2));
    T top = -bottom;
    T left = bottom * aspect;
    T right = -left;

    auto O = ortho(left, right, bottom, top, zNear, zFar);
    return O * A;
}

/**
 * @brief 计算视图矩阵
 *
 * @tparam T
 * @param[in] pos 视点
 * @param[in] origin 视点指向
 * @param[in] up 视线方向
 * @return MatMxN<4, 4, T>
 */
template <typename T>
inline MatMxN<4, 4, T> look_at(const MatMxN<3, 1, T> &pos, const MatMxN<3, 1, T> &origin, const MatMxN<3, 1, T> &up)
{
    // 平移矩阵
    auto Tr = MatMxN<4, 4, T>::identity();
    Tr(0, 3) = -pos.x();
    Tr(1, 3) = -pos.y();
    Tr(2, 3) = -pos.z();

    // 计算相机坐标系
    auto g = (origin - pos).normalize();
    auto x = cross(g, up).normalize();
    auto t = cross(x, g);

    // 旋转矩阵
    auto R = MatMxN<4, 4, T>::identity();
    R(0, 0) = x.x();
    R(0, 1) = x.y();
    R(0, 2) = x.z();

    R(1, 0) = t.x();
    R(1, 1) = t.y();
    R(1, 2) = t.z();

    R(2, 0) = -g.x();
    R(2, 1) = -g.y();
    R(2, 2) = -g.z();

    return R * Tr;
}

/**
 * @brief 组合两个点
 *
 * @tparam N
 * @tparam T
 * @param[in] p1
 * @param[in] p2
 * @param[in] a
 * @param[in] b
 * @return VecT<N, T>
 */
template <int N, class T> inline VecT<N, T> combine(const VecT<N, T> &p1, const VecT<N, T> &p2, T a, T b)
{
    return (p1 * a + p2 * b) / (a + b);
}

/**
 * @brief 点到直线的投影
 *
 * @tparam N
 * @tparam T
 * @param[in] p 目标点
 * @param[in] q 直线上一点
 * @param[in] t 直线方向
 * @return VecT<N, T>
 */
template <int N, class T> inline VecT<N, T> project_to_line(const VecT<N, T> &p, const VecT<N, T> &q, VecT<N, T> t)
{
    if (p == q)
        return p;
    t.normalize();
    return q + t * dot(p - q, t);
}

/**
 * @brief 点到平面的投影
 *
 * @tparam T
 * @param[in] p 目标点
 * @param[in] p1 平面上一点
 * @param[in] n 法向量
 * @return Vec3T<T>
 */
template <std::size_t N, class T>
inline VecT<N, T> project_to_plane(const VecT<N, T> &p, const VecT<N, T> &q, VecT<N, T> n)
{
    if (p == q)
        return p;
    n.normalize();
    auto v = p - q;
    return q + v - n * dot(v, n);
}

/**
 * @brief 点到直线的距离
 *
 * @tparam N
 * @tparam T
 * @param[in] p 目标点
 * @param[in] q 直线上一点
 * @param[in] t 直线方向
 * @return T
 */
template <std::size_t N, class T> inline T distance_to_line(const VecT<N, T> &p, const VecT<N, T> &q, VecT<N, T> t)
{
    return p.distance(project_to_line(p, q, t));
}

/**
 * @brief 点到平面的距离
 *
 * @tparam T
 * @param[in] p 目标点
 * @param[in] q 平面上一点
 * @param[in] n 法向量
 * @return T
 */
template <std::size_t N, class T> inline T distance_to_plane(const VecT<N, T> &p, const VecT<N, T> &q, VecT<N, T> n)
{
    return p.distance(project_to_plane(p, q, n));
}

/**
 * @brief 判断三点是否共线
 *
 * @tparam N
 * @tparam T
 * @param[in] p1
 * @param[in] p2
 * @param[in] p3
 * @return true
 * @return false
 */
template <std::size_t N, class T> inline bool on_line(const VecT<N, T> &p1, const VecT<N, T> &p2, const VecT<N, T> &p3)
{
    if (p2 == p3)
        return true;
    return distance_to_line(p1, p2, p3) <= math::Numeric::distance * (p1 - p2).modulus() * (p1 - p3).modulus();
}

/**
 * @brief 判断四点是否共面
 *
 * @tparam T
 * @param[in] p1
 * @param[in] p2
 * @param[in] p3
 * @param[in] p4
 * @return true
 * @return false
 */
template <std::size_t N, class T>
inline bool on_face(const VecT<N, T> &p1, const VecT<N, T> &p2, const VecT<N, T> &p3, const VecT<N, T> &p4)
{
    auto vec1 = p1 - p2;
    auto vec2 = p1 - p3;
    auto vec3 = p1 - p4;

    // 计算三个向量的行列式，如果足够大，则认为不共面
    return std::abs(dot(cross(vec1, vec2), vec3)) <=
           (math::Numeric::distance * vec1.modulus() * vec2.modulus() * vec3.modulus());
}

/**
 * @brief 求解直线交点（如果不存在，则会返回最小二乘法解）
 *
 * @tparam N
 * @tparam T
 * @param[in] p1
 * @param[in] t1
 * @param[in] p2
 * @param[in] t2
 * @return std::optional<std::tuple<T, T>>
 */
template <std::size_t N, class T>
inline std::optional<std::tuple<T, T>> intersect_line_line(const VecT<N, T> &p1, const VecT<N, T> &t1,
                                                           const VecT<N, T> &p2, const VecT<N, T> &t2)
{
    MatMxN<N, 2, T> A;
    MatMxN<N, 1, T> b;
    for (std::size_t i = 0; i < N; i++)
    {
        A(i, 0) = t1[i];
        A(i, 1) = -t2[i];
        b[i] = p2[i] - p1[i];
    }

    auto ATA = A.transposed() * A;
    auto x = A.transposed() * b;
    bool invertible = math::solve(ATA.data, x.data);
    if (invertible && (p1 + t1 * x[0]).distance(p2 + t2 * x[1]) < math::Numeric::distance)
        return std::make_tuple(x[0], x[1]);

    return std::nullopt;
}

/**
 * @brief 获得平面点在三角形中的重心坐标
 *
 * @tparam N
 * @tparam T
 * @param[in] p
 * @param[in] pts
 * @return std::optional<Vec3T<T>> 重心坐标
 */
template <std::size_t N, class T> inline std::optional<Vec3T<T>> barycentric(const VecT<N, T> &p, const VecT<N, T> *pts)
{
    // 分解 p = (1-u-v)A + uB + vC
    // uAB + vAC + PA = 0
    // [u v 1][ABx ACx PAx]^T = 0
    // [u v 1][ABy ACy PAy]^T = 0
    // u,v 是 右侧两个向量的叉积，并将 z 坐标归一化
    Vec3T<T> u1(pts[2][0] - pts[0][0], pts[1][0] - pts[0][0], pts[0][0] - p[0]);
    Vec3T<T> u2(pts[2][1] - pts[0][1], pts[1][1] - pts[0][1], pts[0][1] - p[1]);
    auto u = cross(u1, u2);

    // 考虑退化情形
    if (std::abs(u.z()) < math::Numeric::distance)
        return std::nullopt;
    return VecT<3, T>(T{1} - (u.x() + u.y()) / u.z(), u.y() / u.z(), u.x() / u.z());
}

} // namespace geom

} // namespace xi
